// Copyright 2024 Polar Bear Team
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "drone_serial_interface/drone_serial_interface_node.hpp"

#include <iomanip>
#include <sstream>
#include <algorithm>
#include <cmath>

namespace drone_serial_interface
{

DroneSerialInterfaceNode::DroneSerialInterfaceNode(const rclcpp::NodeOptions & options)
: rclcpp_lifecycle::LifecycleNode("drone_serial_interface", options),
  serial_connected_(false),
  thread_running_(false)
{
  RCLCPP_INFO(get_logger(), "DroneSerialInterface node initialized");
  
  // Declare parameters
  declareParameters();
}

DroneSerialInterfaceNode::~DroneSerialInterfaceNode()
{
  // Stop thread and close serial connection
  thread_running_ = false;
  if (serial_thread_.joinable()) {
    serial_thread_.join();
  }
  closeSerial();
  
  RCLCPP_INFO(get_logger(), "DroneSerialInterface node destroyed");
}

// ========== Lifecycle Callbacks ==========

rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
DroneSerialInterfaceNode::on_configure(const rclcpp_lifecycle::State & /*state*/)
{
  RCLCPP_INFO(get_logger(), "Configuring drone serial interface...");
  
  // Get parameters
  getParameters();
  
  // Initialize protocol handler
  protocol_handler_ = ProtocolFactory::create(protocol_type_, get_logger());
  if (!protocol_handler_) {
    RCLCPP_ERROR(get_logger(), "Failed to create protocol handler for type: %s", protocol_type_.c_str());
    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE;
  }
  RCLCPP_INFO(get_logger(), "Protocol handler initialized: %s", protocol_handler_->getName().c_str());
  
  // Initialize diagnostic updater
  diagnostic_updater_ = std::make_unique<diagnostic_updater::Updater>(shared_from_this());
  diagnostic_updater_->setHardwareID("drone_serial_interface");
  diagnostic_updater_->add("Connection Status", this, 
    &DroneSerialInterfaceNode::diagnosticConnectionStatus);
  diagnostic_updater_->add("Velocity Status", this, 
    &DroneSerialInterfaceNode::diagnosticVelocityStatus);
  diagnostic_updater_->add("Protocol Status", this, 
    &DroneSerialInterfaceNode::diagnosticProtocolStatus);
  
  // Create publishers
  serial_status_pub_ = create_publisher<std_msgs::msg::String>(
    "serial_status", rclcpp::QoS(10));
  cmd_vel_feedback_pub_ = create_publisher<geometry_msgs::msg::Twist>(
    "cmd_vel_feedback", rclcpp::QoS(10));
  
  // Create subscription
  cmd_vel_sub_ = create_subscription<geometry_msgs::msg::Twist>(
    "cmd_vel", rclcpp::QoS(10),
    std::bind(&DroneSerialInterfaceNode::cmdVelCallback, this, std::placeholders::_1));
  
  // Create timers
  heartbeat_timer_ = create_wall_timer(
    std::chrono::milliseconds(100),
    std::bind(&DroneSerialInterfaceNode::heartbeatTimerCallback, this));
  
  status_timer_ = create_wall_timer(
    std::chrono::seconds(1),
    std::bind(&DroneSerialInterfaceNode::statusTimerCallback, this));
  
  // Initialize serial connection
  if (!initializeSerial()) {
    RCLCPP_ERROR(get_logger(), "Failed to initialize serial connection");
    return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE;
  }
  
  RCLCPP_INFO(get_logger(), "Drone serial interface configured successfully");
  return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
}

// 其余lifecycle函数保持原样...
rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
DroneSerialInterfaceNode::on_activate(const rclcpp_lifecycle::State & /*state*/)
{
  RCLCPP_INFO(get_logger(), "Activating drone serial interface...");
  
  // Start serial communication thread
  thread_running_ = true;
  
  // Activate publishers
  serial_status_pub_->on_activate();
  cmd_vel_feedback_pub_->on_activate();
  
  RCLCPP_INFO(get_logger(), "Drone serial interface activated");
  return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
}

// ========== 更新的串口通信函数 - 使用协议处理器 ==========

bool DroneSerialInterfaceNode::sendVelocityCommand(const geometry_msgs::msg::Twist & cmd_vel)
{
  if (!serial_port_ || !serial_port_->isOpen() || !protocol_handler_) {
    return false;
  }
  
  try {
    // 使用协议处理器编码命令
    std::string command = protocol_handler_->encode(cmd_vel);
    
    // 验证编码后的数据
    ProtocolStatus status = protocol_handler_->validate(command);
    if (!status.is_valid) {
      RCLCPP_ERROR(get_logger(), "Protocol validation failed: %s", status.error_message.c_str());
      return false;
    }
    
    // 发送命令
    size_t bytes_written = serial_port_->write(command);
    
    if (bytes_written != command.length()) {
      RCLCPP_WARN(get_logger(), 
        "Incomplete write: %zu/%zu bytes", bytes_written, command.length());
      return false;
    }
    
    RCLCPP_DEBUG(get_logger(), "Sent %s command: %s", 
      protocol_handler_->getName().c_str(), command.c_str());
    return true;
    
  } catch (const serial::IOException & e) {
    RCLCPP_ERROR(get_logger(), "Serial write error: %s", e.what());
    serial_connected_ = false;
    return false;
  }
}

// 移除旧的formatVelocityCommand函数，现在由协议处理器处理

} // namespace drone_serial_interface